using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using DemoContentLoader;
using DemoRenderer;
using DemoRenderer.UI;
using DemoUtilities;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace Demos.Demos
{
    /// <summary>
    /// 显示了一种处理需要接触级测试精度的冲突查询的方法。
    /// </summary>
    public class CollisionQueryDemo : Demo
    {
        public override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(0, 7, 20);
            camera.Yaw = 0;
            camera.Pitch = 0;

            // PositionFirstTimestpper是最简单的时步模式,但由于它在帧开始时将速度积分到位置中,因此直接修改了时间步长之外的速度
            // 将在碰撞检测或解算器有机会介入之前集成。这在这个演示中很好。其他内置选项包括PositionLastTimestpper和SubsteppingTimestpper。
            // 请注意,timeSteper还具有回调,您可以使用这些回调在处理阶段之间执行逻辑,如BeforeCollisionDetection。
            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());

            Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 1, 100)), 0.1f)));

            var random = new Random(5);
            var shapeToDrop = new Box(1, 1, 1);
            shapeToDrop.ComputeInertia(1, out var shapeToDropInertia);
            var descriptionToDrop = BodyDescription.CreateDynamic(new Vector3(), shapeToDropInertia, new CollidableDescription(Simulation.Shapes.Add(shapeToDrop), 0.3f), new BodyActivityDescription(0.01f));
            for (int i = 0; i < 128; ++i)
            {
                descriptionToDrop.Pose.Position = new Vector3(-5 + 10 * (float)random.NextDouble(), 45 + 150 * (float)random.NextDouble(), -5 + 10 * (float)random.NextDouble());
                Simulation.Bodies.Add(descriptionToDrop);
            }

            // 添加要测试的静电对象。请注意,网格三角形是单边的,因此中心位于网格下方的某些查询不会生成任何接触。
            DemoMeshHelper.CreateDeformedPlane(20, 20, (x, y) => { return new Vector3(x * 5 - 50, 3 * MathF.Sin(x) * MathF.Sin(y), y * 5 - 50); }, Vector3.One, BufferPool, out var mesh);
            Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f)));
        }

        /// <summary>
        /// 提供对CollisionBatcher的筛选和数据收集回调,我们将使用该回调根据检测到的环境测试查询形状。
        /// </summary>
        public struct BatcherCallbacks : ICollisionCallbacks
        {
            /// <summary>
            /// 如果碰撞测试检测到非负深度,则将对设置为true。
            /// </summary>
            public Buffer<bool> QueryWasTouched;

            // 这些回调为冲突批处理程序正在处理的对提供过滤和报告。
            // "配对ID"是指在将配对添加到批次时给予该配对的标识符。
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool AllowCollisionTesting(int pairId, int childA, int childB)
            {
                // 如果您想要基于遇到的非凸对象的子对象进行过滤,这里将是一个很好的地方。
                // pairId可用于查找涉及的对象和过滤所需的任何元数据。
                return true;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void OnChildPairCompleted(int pairId, int childA, int childB, ref ConvexContactManifold manifold)
            {
                // 如果在子流形返回到非凸面处理过程之前需要对子流形执行任何处理,则可以在此处执行。
                // 凸凸对根本不会调用此函数。
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void OnPairCompleted<TManifold>(int pairId, ref TManifold manifold) where TManifold : unmanaged, IContactManifold<TManifold>
            {
                // 此函数将完成的流形与所有后处理(非凸面简化、网格简化等)进行交接。完成了。
                // 出于本演示的目的,我们对布尔冲突测试感兴趣。
                // (这个过程对于纯布尔测试来说有点过头了,但是现在没有纯布尔路径,因为接触流形生成器产生的速度足够快。
                // 如果您发现自己需要联系数据,那么,您可以在手边找到它。)
                for (int i = 0; i < manifold.Count; ++i)
                {
                    // 这看起来可能有点奇怪,但它解决了返回对结构"this"实例的引用的限制。
                    // (我们在这里真正想要的是取消该限制,或者允许接口要求静电成员,这样我们就可以调用静电函数并传递实例,
                    // 而不是调用实例上的函数并传递实例。)
                    if (manifold.GetDepth(ref manifold, i) >= 0)
                    {
                        QueryWasTouched[pairId] = true;
                        break;
                    }
                }
            }
        }

        /// <summary>
        /// 由BroadPhase.GetOverlaps调用以收集遇到的所有可碰撞对象。
        /// </summary>
        struct BroadPhaseOverlapEnumerator : IBreakableForEach<CollidableReference>
        {
            public QuickList<CollidableReference> References;
            // 枚举数永远不会存储到非托管内存中,因此包含引用类型实例是安全的。
            public BufferPool Pool;
            public bool LoopBody(CollidableReference reference)
            {
                References.Allocate(Pool) = reference;
                // 如果您想要进行任何顶级过滤,这里将是一个很好的位置。
                // CollidableReference告诉您它是一个Body还是一个静电对象以及相关的句柄。您可以用它查找元数据。
                return true;
            }
        }


        void GetPoseAndShape(CollidableReference reference, out RigidPose pose, out TypedIndex shapeIndex)
        {
            // 可碰撞物体可以与物体或静力学相关联。我们必须根据不同的地方寻找不同的地方。
            if (reference.Mobility == CollidableMobility.Static)
            {
                var staticIndex = Simulation.Statics.HandleToIndex[reference.StaticHandle.Value];
                pose = Simulation.Statics.Poses[staticIndex];
                shapeIndex = Simulation.Statics.Collidables[staticIndex].Shape;
            }
            else
            {
                var bodyReference = Simulation.Bodies.GetBodyReference(reference.BodyHandle);
                pose = bodyReference.Pose;
                shapeIndex = bodyReference.Collidable.Shape;
            }
        }

        /// <summary>
        /// 将形状查询添加到碰撞批处理程序。
        /// </summary>
        /// <param name="queryShapeType">要测试的形状的类型。</param>
        /// <param name="queryShapeData">要测试的形状数据。</param>
        /// <param name="queryShapeSize">形状数据的大小(字节)。</param>
        /// <param name="queryBordsMin">查询形状的边框的最小值。</param>
        /// <param name="query sMax">查询形状的边框的最大值。</param>
        /// <param name="queryPose">查询形状的姿势。</param>
        /// <param name="queryId">冲突批处理程序完成处理时用于引用此查询的ID。</param>
        /// <param name="Batcher">要将查询的测试添加到的批处理程序。</param>
        public unsafe void AddQueryToBatch(int queryShapeType, void* queryShapeData, int queryShapeSize, in Vector3 queryBoundsMin, in Vector3 queryBoundsMax, in RigidPose queryPose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher)
        {
            var broadPhaseEnumerator = new BroadPhaseOverlapEnumerator { Pool = BufferPool, References = new QuickList<CollidableReference>(16, BufferPool) };
            Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, ref broadPhaseEnumerator);
            for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex)
            {
                GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex);
                Simulation.Shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _);
                // 在此路径中,我们假设传入的形状数据是短暂的。冲突批处理程序可能比数据指针持续更长时间。
                // 为了避免未定义的访问,我们将查询数据缓存到冲突批处理程序中,并使用指向缓存的指针。
                batcher.CacheShapeB(shapeIndex.Type, queryShapeType, queryShapeData, queryShapeSize, out var cachedQueryShapeData);
                batcher.AddDirectly(
                    shapeIndex.Type, queryShapeType,
                    shapeData, cachedQueryShapeData,
                    // 因为我们将其用作布尔查询,所以我们使用的推测边距为0。不要在意负面的深度。
                    queryPose.Position - pose.Position, queryPose.Orientation, pose.Orientation, 0, new PairContinuation(queryId));
            }
            broadPhaseEnumerator.References.Dispose(BufferPool);
        }

        /// <summary>
        /// 将形状查询添加到碰撞批处理程序。
        /// </summary>
        /// <typeparam name="TShape">查询形状的类型。</typeparam>
        /// <param name="Shape">要在查询中使用的形状。</param>
        /// <param name="pose">查询形状的姿势。</param>
        /// <param name="queryId">冲突批处理程序完成处理时用于引用此查询的ID。</param>
        /// <param name="Batcher">要将查询的测试添加到的批处理程序。</param>
        public unsafe void AddQueryToBatch<TShape>(TShape shape, in RigidPose pose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher) where TShape : IConvexShape
        {
            var queryShapeData = Unsafe.AsPointer(ref shape);
            var queryShapeSize = Unsafe.SizeOf<TShape>();
            shape.ComputeBounds(pose.Orientation, out var boundingBoxMin, out var boundingBoxMax);
            boundingBoxMin += pose.Position;
            boundingBoxMax += pose.Position;
            AddQueryToBatch(shape.TypeId, queryShapeData, queryShapeSize, boundingBoxMin, boundingBoxMax, pose, queryId, ref batcher);
        }

        // 此版本的查询未在演示中使用,但显示了如何在查询中使用模拟缓存形状。
        /// <summary>
        /// 将形状查询添加到碰撞批处理程序。
        /// </summary>
        /// <typeparam name="TShape">查询形状的类型。</typeparam>
        /// <param name="Shape">要在查询中使用的形状。</param>
        /// <param name="pose">查询形状的姿势。</param>
        /// <param name="queryId">冲突批处理程序完成处理时用于引用此查询的ID。</param>
        /// <param name="Batcher">要将查询的测试添加到的批处理程序。</param>
        public unsafe void AddQueryToBatch(Shapes shapes, TypedIndex queryShapeIndex, in RigidPose queryPose, int queryId, ref CollisionBatcher<BatcherCallbacks> batcher)
        {
            var shapeBatch = shapes[queryShapeIndex.Type];
            shapeBatch.ComputeBounds(queryShapeIndex.Index, queryPose, out var queryBoundsMin, out var queryBoundsMax);
            Simulation.Shapes[queryShapeIndex.Type].GetShapeData(queryShapeIndex.Index, out var queryShapeData, out _);
            var broadPhaseEnumerator = new BroadPhaseOverlapEnumerator { Pool = BufferPool, References = new QuickList<CollidableReference>(16, BufferPool) };
            Simulation.BroadPhase.GetOverlaps(queryBoundsMin, queryBoundsMax, ref broadPhaseEnumerator);
            for (int overlapIndex = 0; overlapIndex < broadPhaseEnumerator.References.Count; ++overlapIndex)
            {
                GetPoseAndShape(broadPhaseEnumerator.References[overlapIndex], out var pose, out var shapeIndex);
                // 因为两个涉及的形状都来自模拟缓存,所以我们不需要自己缓存它们。
                Simulation.Shapes[shapeIndex.Type].GetShapeData(shapeIndex.Index, out var shapeData, out _);
                batcher.AddDirectly(
                    shapeIndex.Type, queryShapeIndex.Type,
                    shapeData, queryShapeData,
                    // 因为我们将其用作布尔查询,所以我们使用的推测边距为0。不要在意负面的深度。
                    queryPose.Position - pose.Position, queryPose.Orientation, pose.Orientation, 0, new PairContinuation(queryId));
            }
            broadPhaseEnumerator.References.Dispose(BufferPool);
        }

        // 对于演示,我们将使用一组框作为查询。
        struct Query
        {
            public Box Box;
            public RigidPose Pose;
        }

        public unsafe override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
        {
            // 碰撞批处理程序一次对多个测试进行矢量化,因此为了获得最佳性能,您需要向它提供一系列测试。
            var collisionBatcher = new CollisionBatcher<BatcherCallbacks>(BufferPool, Simulation.Shapes, Simulation.NarrowPhase.CollisionTaskRegistry, 0, new BatcherCallbacks());

            // 预先创建一组查询。我们存储它们,这样我们就可以在事后根据它们的触摸状态用颜色来呈现它们。
            const int widthInQueries = 5;
            var queries = new QuickList<Query>(widthInQueries * widthInQueries, BufferPool);
            var querySpacing = new Vector3(3, 0, 3);
            var basePosition = new Vector3(0, 2, 0) - new Vector3(widthInQueries - 1) * querySpacing * 0.5f;
            for (int i = 0; i < 5; ++i)
            {
                for (int j = 0; j < 5; ++j)
                {
                    queries.Allocate(BufferPool) = new Query { Box = new Box(1, 1, 1), Pose = new RigidPose(basePosition + querySpacing * new Vector3(i, 0, j)) };
                }
            }
            // 请注意,回调和set都是值类型,因此在复制时必须稍微小心一些。
            ref var queryWasTouched = ref collisionBatcher.Callbacks.QueryWasTouched;
            BufferPool.Take(queries.Count, out queryWasTouched);
            queryWasTouched.Clear(0, queryWasTouched.Length);

            for (int queryIndex = 0; queryIndex < queries.Count; ++queryIndex)
            {
                ref var query = ref queries[queryIndex];
                AddQueryToBatch(query.Box, query.Pose, queryIndex, ref collisionBatcher);
            }
            // 虽然如果冲突批处理机填充了批,则当添加新的测试时它可以在这里和那里刷新批,
            // 很可能还有剩余的配对没有完全填满一批。强制完全冲洗。
            // 请注意,这还会将批处理程序使用的所有资源返回到BufferPool。
            collisionBatcher.Flush();

            // 使用适当的颜色呈现查询框。
            for (int i = 0; i < queries.Count; ++i)
            {
                ref var query = ref queries[i];
                renderer.Shapes.AddShape(query.Box, Simulation.Shapes, ref query.Pose, queryWasTouched[i] ? new Vector3(0, 1, 0) : new Vector3(1, 0, 0));
            }

            BufferPool.Return(ref queryWasTouched);
            queries.Dispose(BufferPool);
        }
    }
}

